/*
 *  Equalizer.c
 *  MantuPlay
 *
 *  Created by Markus Sintonen on 1.11.2009.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

#include "equalizer.h"
#include <math.h>
#include <limits>

const double Q = 1.2247449; // Q value for band-pass filters 1.2247=(3/2)^(1/2) gives 4dB suppression @ Fc*2 and Fc/2

EqualizerSettings::EqualizerSettings() 
{
	freq[0] = 31.25f;
	for (int i = 1; i < NUMBER_OF_BANDS; i++) {
		freq[i] = freq[i-1] * 2;
	}
	for(int i = 0; i < NUMBER_OF_BANDS; i++) {
		eq[i] = 0.5f;
	}
	//eq[0] = 0.9f;
	//eq[1] = 0.7f;
	lowdb = -100;
	highdb = 100;
}

void Equalizer::bp2(float* a, float* b, double fc, double q)
{
	double th = 2*M_PI*fc;
	double C = (1 - tan(th*q/2))/(1 + tan(th*q/2));
	static const double bp2mult = 1;
	
	a[0] = float(bp2mult*(1 + C)*cos(th));
	a[1] = float(bp2mult*-1*C);
	
	b[0] = float(bp2mult*(1.0 - C)/2.0);
	b[1] = float(bp2mult*-1.0050);
}
Equalizer::Equalizer(const EqualizerSettings *cfg, double sampleRate)
{
	reset();
	static const double qmult=1;
	for(int i = 0; i < AF_NCH; i++)
		for(int j = 0; j < KM; j++)
		{
			float db = ((float)(cfg->highdb - cfg->lowdb))*cfg->eq[j] + (float)cfg->lowdb;
			g[i][j] = (float)(qmult*(powf(10.0f, db/2000.0f) - 1.0));
		}
	
	K = KM;
	while (cfg->freq[K-1] > sampleRate/2.3)
		K--;
	for (int k = 0; k < K; k++)
		bp2(a[k], b[k], cfg->freq[k]/sampleRate, Q);
}

#pragma arm
float Equalizer::equalize(const float *g, SInt16 in, unsigned int ci)
{
	float yt = (float)in;
	for (int k = 0; k < K; k++)
	{
		float *wq = this->wq[ci][k];
		float wq0 = wq[0];
		float wq1 = wq[1];
		float w = yt*b[k][0] + wq0*a[k][0] + wq1*a[k][1];
		yt += (w + wq1*b[k][1])*g[k];
		wq[1] = wq0;
		wq[0] = w;
	}
	return yt;
}
#pragma thumb

void Equalizer::process(SInt32 *samples, UInt32 numsamples, unsigned int nchannels)
{
	SInt16 int16max = std::numeric_limits<SInt16>::max();
	SInt16 int16min = std::numeric_limits<SInt16>::min();
	unsigned int ci = nchannels;
	while (ci--)
	{
		const float *g = this->g[ci];
		SInt16 *inout = (SInt16*)samples + ci;
		SInt16 *end = inout + nchannels*numsamples;
		while (inout<end)
		{
			float yt = equalize(g, *inout, ci);
			if(yt < int16min)
				*inout = int16min;
			else if(yt > int16max)
				*inout = int16max;
			else
				*inout = (SInt16)yt;
			inout += nchannels;
		}
	}
}